// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ExternalDatasetSLAMOutputFileReader.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections.Concurrent;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// The file reader that can read the published results of an external slam implementation against an external dataset
    /// </summary>
    public class ExternalDatasetSLAMOutputFileReader : IFileReader
    {
        /// <summary>
        /// The file to open and read
        /// </summary>
        private string filePath;

        /// <summary>
        /// The queue of deserialized messages waiting to be published at the right time
        /// </summary>
        private BlockingCollection<AgentMessage> publishingQueue;

        /// <summary>
        /// Indicates if the reader has more messages to publish.
        /// </summary>
        private bool isActive;

        /// <summary>
        /// Initializes a new instance of the ExternalDatasetSensorFileReader class
        /// </summary>
        /// <param name="filePath">The file path</param>
        /// <param name="publishingQueue">The queue where to store the messages deserialized so far</param>
        /// <returns>The recording start time, if persisted in the file</returns>
        public long Initialize(string filePath, BlockingCollection<AgentMessage> publishingQueue)
        {
            this.publishingQueue = publishingQueue;
            this.filePath = filePath;
            this.isActive = true;
            return 0;
        }

        /// <summary>
        /// Deserializes messages and inserts them in the publishing queue.
        /// This method blocks until Stop is called or until the end of file is reached.
        /// </summary>
        public void Start()
        {
            int mapSize = 900;
            int startX = mapSize / 2;
            int startY = mapSize * 3 / 4;
            float resolution = 0.1f;
            int maxDepth = 50;
            int localMapSize = (int)(maxDepth / resolution);

            using (StreamReader fileReader = new StreamReader(this.filePath))
            {
                GlobalMap map = new GlobalMap(mapSize, mapSize, new Pose2D(startX, startY, 0), resolution);
                Pose2D likelyPose = new Pose2D();

                // loop over all entries in the file and add them to the queue.
                // the queue will block if we are going too fast.
                while (!fileReader.EndOfStream && this.isActive)
                {
                    string line = fileReader.ReadLine();
                    if (string.IsNullOrEmpty(line))
                    {
                        continue;
                    }

                    string[] parts = line.Split();

                    if (parts[0] == "ODO_UPDATE")
                    {
                        long originatingTime = (long)(double.Parse(parts[parts.Length - 1]) * 1000);
                        
                        int particleCount = int.Parse(parts[1]);
                        double x = double.Parse(parts[2]);
                        double y = double.Parse(parts[3]);
                        double heading = double.Parse(parts[4]);
                        double weight = double.Parse(parts[5]);
                        likelyPose = new Pose2D(x, y, heading);

                        SLAM2DAgentMessage message = new SLAM2DAgentMessage(map, likelyPose, new[] { likelyPose }, null, originatingTime);
                        message.Timestamp = originatingTime;
                        this.publishingQueue.Add(message);
                    }
                    else if (parts[0] == "LASER_READING")
                    {
                        SimpleAgentMessage<LaserHorizontalDepthProfile> hdp = ExternalDatasetSensorFileReader.ParseHDPMessage(parts);
                        Pose2D mapPose = new Pose2D(likelyPose.X / resolution + startX, likelyPose.Y / resolution + startY, likelyPose.Heading);
                        map.Update(hdp.State, mapPose);
                    }
                }
            }

            this.publishingQueue.CompleteAdding();
        }

        /// <summary>
        /// Stops listening for messages
        /// </summary>
        public void Stop()
        {
            this.isActive = false;
        }
    }
}
